#include <ros/ros.h>
#include <fstream>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>

#include <tf/transform_broadcaster.h>

#include "eigen_conversions/eigen_msg.h"
#include <geometry_msgs/PoseStamped.h>
#include <Eigen/Core>
#include <Eigen/SVD>
#include <Eigen/Cholesky>
#include <Eigen/Geometry>
#include <Eigen/LU>
#include "path_planner/PathPlanner.h"

using namespace Eigen;
using namespace std;


tf::TransformBroadcaster *tf_uav_enu_pub_ptr;
tf::TransformBroadcaster *tf_posesp_enu_pub_ptr;
bool uav_pose_inited = false;
Affine3d uav_init_pose;


void uavposePoseCallback(const geometry_msgs::PoseStamped::ConstPtr& msg);

void BroadcastTF(tf::TransformBroadcaster * tfBroadcaster_ptr, const Affine3d& pose,const char* world, const char* tag)
{
	Eigen::Quaterniond pose_R_quat = Eigen::Quaterniond(pose.rotation());
	Eigen::Vector3d pose_t = pose.translation();
	tf::Transform tf_uav_enu;
	tf_uav_enu.setOrigin(tf::Vector3(pose_t.x(),pose_t.y(),pose_t.z()));
	tf_uav_enu.setRotation(tf::Quaternion(
			pose_R_quat.x(),
			pose_R_quat.y(),
			pose_R_quat.z(),
			pose_R_quat.w()
	));
	tfBroadcaster_ptr->sendTransform(tf::StampedTransform(tf_uav_enu,ros::Time::now(), world, tag));
}

mavros_msgs::State current_state;



void state_cb(const mavros_msgs::State::ConstPtr& msg){
    current_state = *msg;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "offboard_controller");
    ros::NodeHandle nh;

    tf_uav_enu_pub_ptr = new tf::TransformBroadcaster();
	tf_posesp_enu_pub_ptr = new tf::TransformBroadcaster();

	ros::Subscriber uavpose_sub = nh.subscribe("/mavros/local_position/pose", 10, uavposePoseCallback);


    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
            ("mavros/state", 10, state_cb);
    ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
            ("mavros/setpoint_position/local", 10);
    ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
            ("mavros/cmd/arming");
    ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
            ("mavros/set_mode");

    //the setpoint publishing rate MUST be faster than 2Hz
    const double cycle_rate = 25.0;
    const double cycle_interval = 1.0 / cycle_rate;
    ros::Rate rate(cycle_rate);

    // wait for FCU connection
    while(ros::ok() && current_state.connected){
        ros::spinOnce();
        rate.sleep();
    }

    Affine3d pose_target = Affine3d::Identity();

    pose_target.translation().x() = 0;
    pose_target.translation().y() = 0;
    pose_target.translation().z() = 0.5;

    geometry_msgs::PoseStamped pose_target_msg;


    mavros_msgs::CommandBool arm_cmd;
    arm_cmd.request.value = false;
    ros::Time last_request = ros::Time::now();

	string file_name = "/home/dozen/path_test.txt";
	ros::param::get("~file_name", file_name);
	ros::param::del("~file_name");


    ifstream path_ifs(file_name.c_str());

    ROS_INFO("Loading path plan file %s",file_name.c_str());

    if(!path_ifs.is_open())
    {
    	ROS_ERROR("OPEN FILE FAILED");
    	return 1;
    }
    PathPlanner pPlanner;
    bool pPlanner_started = false;
    char command_line[256];
    bool end_of_file = false;

    //uav_pose_inited=true;
    //uav_init_pose = Affine3d::Identity();

    while(ros::ok()){
    	/*
        if( current_state.mode != "OFFBOARD" &&
            (ros::Time::now() - last_request > ros::Duration(5.0))){
            if( set_mode_client.call(offb_set_mode) &&
                offb_set_mode.response.success){
                ROS_INFO("Offboard enabled");
            }
            last_request = ros::Time::now();
        } else {

            if( !current_state.armed &&
                (ros::Time::now() - last_request > ros::Duration(5.0))){
                if( arming_client.call(arm_cmd) &&
                    arm_cmd.response.success){
                    ROS_INFO("Vehicle armed");
                }
                last_request = ros::Time::now();
            }

        }
        */
    	if(!uav_pose_inited)
    	{
            ros::spinOnce();
            rate.sleep();
            continue;
    	}
    	if(!pPlanner_started)
    	{
    		double init_x = uav_init_pose.translation().x();
    		double init_y = uav_init_pose.translation().y();
    		double init_z = uav_init_pose.translation().z();
    		AngleAxisd init_rotation = AngleAxisd(uav_init_pose.rotation());
    		double init_yaw = init_rotation.angle();
    		Vector3d init_yaw_axis = init_rotation.axis();
    		ROS_INFO("%lf %lf %lf",init_yaw_axis.x(),init_yaw_axis.y(), init_yaw_axis.z());
    		if(init_yaw_axis.z() > 0.01)
    		{
    			init_yaw = -init_yaw;
    		}
    		else if(init_yaw_axis.z() < -0.01)
    		{

    		}
    		else
    		{
    			init_yaw = 0;
    		}
    		if(init_yaw < 0) init_yaw += M_PI*2; if(init_yaw < 0) init_yaw += M_PI*2;
    		if(init_yaw > M_PI*2) init_yaw -= M_PI*2; if(init_yaw > M_PI*2) init_yaw -= M_PI*2;
    		init_yaw = init_yaw*180/M_PI;

    		ROS_INFO("init at %lf %lf %lf %lf ",init_x, init_y, init_z, init_yaw);
    	    pPlanner.TakeActionScript(PathPlanner::START,2.0,init_x, init_y, init_z, init_yaw);
    		pPlanner_started=true;
    	}

    	if(pPlanner.time_left <=0 && !end_of_file && pPlanner_started)
    	{
    		bool line_valid=false;
    		for(int ci=0;ci<256;ci++)
    		{
    			path_ifs.read(&command_line[ci],1);
    			if(command_line[ci] == '\n')
    			{
    				command_line[ci] = '\0';
    				line_valid=true;
    				break;
    			}
    			if(path_ifs.eof())
    			{
    				end_of_file=true;
    				break;
    			}
    		}
    		if(line_valid)
    		{
    			ROS_INFO("[command_line] %s",command_line);
    			pPlanner.TakcActionFromString(command_line);

    		}
    	}
    	else
    	{

    		if(pPlanner.time_left >0)
    		{
    			pPlanner.Step(cycle_interval);
    		}
    	}
    	pose_target=pPlanner.GetSetPointEigen();
    	//cout<<pose_target.matrix()<<endl;

    	pose_target_msg.header.stamp = ros::Time::now();
    	tf::poseEigenToMsg(pose_target,pose_target_msg.pose);
        local_pos_pub.publish(pose_target_msg);
        BroadcastTF(tf_posesp_enu_pub_ptr,pose_target,"ENU","UAV_TARGET");

        ros::spinOnce();
        rate.sleep();
    }
    return 0;
}

void uavposePoseCallback(const geometry_msgs::PoseStamped::ConstPtr& msg)
{
	Affine3d uav_local_pose;
	tf::poseMsgToEigen(msg->pose,uav_local_pose);
	BroadcastTF(tf_uav_enu_pub_ptr,uav_local_pose,"ENU", "UAV");

	if(!uav_pose_inited)
	{
		uav_init_pose=uav_local_pose;
		uav_pose_inited = true;
	}

}



